﻿//////////////////////////////////////////////////////////////////////////////////
//
//  Game Demo 
//  @ This demo shows some simple editing of level & vehicle
//  @ Team Twilight (2009)
//  @ Last Updated: 3.25am, 18 Feb 09
//  @ See ChangeLog.txt for revisions
//////////////////////////////////////////////////////////////////////////////////
using System.Text;
using System;
using System.IO;
using System.Diagnostics;
using FarseerGames.AdvancedSamples.DrawingSystem;
using FarseerGames.AdvancedSamples.ScreenSystem;
using FarseerGames.FarseerPhysics;
using FarseerGames.FarseerPhysics.Collisions;
using FarseerGames.FarseerPhysics.Dynamics;
using FarseerGames.FarseerPhysics.Dynamics.Springs;
using FarseerGames.FarseerPhysics.Factories;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Graphics;
using Microsoft.Xna.Framework.Input;
using FarseerGames.AdvancedSamples.Demos.DemoShare;
using FarseerGames.FarseerPhysics.Dynamics.Joints;
using FarseerGames.FarseerPhysics.Mathematics;
using System.Collections.Generic;


namespace FarseerGames.AdvancedSamples.Demos.Demo6
{
    public class Demo6Screen : GameScreen
    {
        #region Variables Declaration
        // shapes
        private BodyList _obstacles;
        private GeomList _obstaclesg;

        //@kester - motors and joints
        private BodyList _motorCWBodyList;
        private GeomList _motorCWGeomList;
        private BodyList _motorACWBodyList;
        private GeomList _motorACWGeomList;
        private BodyList _wheelBodyList;
        private GeomList _wheelGeomList;
        private bool _flagStartSim = false;
        private bool _flagDoJoint = false;
        private RectangleBrush _rectBrush = new RectangleBrush(800, 10, Color.Green, Color.Black);
        private GeomList _selectedGeoms = new GeomList();
        private Geom _pickedGeom2;
        private Texture2D _wheelTexture;
        private Vector2 _wheelOrigin;
        private Texture2D _motorCWTexture; // clockwise
        private Texture2D _motorACWTexture; // anticlockwise
        private float _torqueMotor = 0.0f;
        private Texture2D _shaftTextureIcon;
        private Vector2 _shaftOriginIcon;
        private Vector2 _shaftScale;
        private Vector2 _shaftDifference;
        private Vector2 _normalizedDifference = Vector2.Zero;
        private float _shaftRotation;
        private float _shaftTheta;
        private Vector2 _shaftxVector = new Vector2(1, 0);
        private bool _motorDirection = true;
        private BodyList _shaftBodyList = new BodyList();
        private GeomList _shaftGeomList;
        private Texture2D _shaftTexture;
        private Vector2 _shaftOrigin;
        private RevoluteJoint _jointRev;
        private JointList _jointRevList = new JointList();
        private List<GeomPair> _shaftGeomPairList = new List<GeomPair>();
        enum ScreenMode { LEditor = 1, VEditor, Game };
        private int _currScreenMode = (int) ScreenMode.LEditor;
        private StreamWriter _vehicleSW;
        private StreamWriter _levelSW;
        private StreamReader _vehicleSR;
        private StreamReader _levelSR;
        private StreamReader _levelCountSR;
        private StreamReader _vehicleCountSR;
        private StreamWriter _debugSW;
        private int _totalMapCount = 0;
        private int _totalVehicleCount = 0;
        private int _mapID = 3;
        private int _vehicleID = 1;
        private int _geom1Id = 0; // body1 id connected to shaft 
        private int _geom2Id = 0; // body2 id connected to shaft
        private int _shaftID = 0; // shaft id
        private string _bodiesId = "";
        private float _shaftWidth = 0.0f;

        private Vector2 point; // mouse position

        // shapes
        private Texture2D _squaresTexture;
        private Vector2 _squaresOrigin;
        private Texture2D _circlesTexture;
        private Vector2 _circlesOrigin;
        private Texture2D _vOblongsTexture;
        private Vector2 _vOblongsOrigin;
        private Texture2D _hOblongsTexture;
        private Vector2 _hOblongsOrigin;
        private Texture2D _polygonTexture;
        private Vector2 _polygonOrigin;
        private Vector2 _dynamicOrigin;

        //@jack
        private Vector2 deltaMovement;
        private Vector2 position;
        private int CursorSpeed = 700;
        //@end jack

        private Border _border;
        private LineBrush _lineBrush = new LineBrush(1, Color.Black); //used to draw spring on mouse grab
        private FixedLinearSpring _mousePickSpring;
        private Geom _pickedGeom;

        // custom variables
        Vector2 itemPos = new Vector2(900, 100); // item icons position 
        private int shape;  // assigns current shape to newly created body
        private bool stat;  // placing static object?
        private int placeMode = 1;  // 1-free to move, 2-static, 3-activatable static
        private int maxShapes = 6;  // update this whenever shapes are added
        private Body dragged;   // holds current dragged body
        private Vertices verts; // container for texture-read images
        private Vertices vertices;  // dynamic object vertices on creation
        private bool firstClick = true; // first click of dynamic object creation
        EffectPool effectPool;
        BasicEffect basicEffect;
        VertexPositionColor[] vertices3 = new VertexPositionColor[1000];   // for drawing in filled colours
        private int vertices3Counter = 0;
        private int triangleCount = 0;
        private Vector2 pointCheck = new Vector2(0, 0);
        String str = "";
        String _strScreenMode = "";
        

        #endregion

        #region Initialize Method

        public override void Initialize()
        {
            PhysicsSimulator = new PhysicsSimulator(new Vector2(0, 100));
            PhysicsSimulatorView = new PhysicsSimulatorView(PhysicsSimulator);

            //ScreenManager.GraphicsDevice.VertexDeclaration = new VertexDeclaration(ScreenManager.GraphicsDevice, VertexPositionColorTexture.VertexElements);
            //ScreenManager.GraphicsDevice.RenderState.CullMode = CullMode.None;

            // Create default shading procedure
            effectPool = new EffectPool();
            basicEffect = new BasicEffect(ScreenManager.GraphicsDevice, effectPool);
            basicEffect.VertexColorEnabled = true;
            

            for (int i = 0; i < 1000; i++)
            {
                if(i%3 == 0)
                    vertices3[i].Color = Color.White;
                
                if (i % 3 == 1)
                    vertices3[i].Color = Color.Red;

                if (i % 3 == 2)
                    vertices3[i].Color = Color.Blue;
            }

            base.Initialize();
        }

        #endregion
      

        #region LoadContent Method
        public override void LoadContent()
        {

            // custom shapes
            _squaresTexture = DrawingHelper.CreateRectangleTexture(ScreenManager.GraphicsDevice, 40, 40, Color.LawnGreen,
                                                                    Color.Black);
            _circlesTexture = DrawingHelper.CreateCircleTexture(ScreenManager.GraphicsDevice, 20, 2, Color.Azure,
                                                                   Color.Black);
            _vOblongsTexture = DrawingHelper.CreateRectangleTexture(ScreenManager.GraphicsDevice, 20, 80, Color.Bisque,
                                                                    Color.Black);
            _hOblongsTexture = DrawingHelper.CreateRectangleTexture(ScreenManager.GraphicsDevice, 80, 20, Color.MediumAquamarine,
                                                                    Color.Black);
            
            _polygonTexture = ScreenManager.ContentManager.Load<Texture2D>("Content/Slope");
            //Create an array to hold the data from the texture
            uint[] data = new uint[_polygonTexture.Width * _polygonTexture.Height];
            //Transfer the texture data to the array
            _polygonTexture.GetData(data);
            //Calculate the vertices from the array
            verts = Vertices.CreatePolygon(data, _polygonTexture.Width, _polygonTexture.Height);
            //Make sure that the origin of the texture is the centroid (real center of geometry)
            _polygonOrigin = verts.GetCentroid();     

            //@kester - motor & joints textures origin
            _wheelTexture = DrawingHelper.CreateCircleTexture(ScreenManager.GraphicsDevice, 34, 3, Color.White, Color.Black);
            _wheelOrigin = new Vector2(_wheelTexture.Width / 2f, _wheelTexture.Height / 2f);
            _motorCWTexture = DrawingHelper.CreateCircleTexture(ScreenManager.GraphicsDevice, 34, 3, Color.Blue, Color.Black);
            _motorACWTexture = DrawingHelper.CreateCircleTexture(ScreenManager.GraphicsDevice, 34, 3, Color.Red, Color.Black);
            _wheelOrigin = new Vector2(_motorCWTexture.Width / 2f, _motorCWTexture.Height / 2f);   
            _shaftTextureIcon = ScreenManager.ContentManager.Load<Texture2D>("Content/JointIcon");
            _shaftOriginIcon = new Vector2(_shaftTextureIcon.Width / 2f, _shaftTextureIcon.Height / 2f);            
            _shaftTexture = DrawingHelper.CreateRectangleTexture(ScreenManager.GraphicsDevice, 1000, 15, Color.Green, Color.Black);
            _shaftOrigin = new Vector2(_shaftTexture.Width / 2f, _shaftTexture.Height / 2f + 1);

            _squaresOrigin = new Vector2(_squaresTexture.Width / 2f, _squaresTexture.Height / 2f);
            _circlesOrigin = new Vector2(_circlesTexture.Width / 2f, _circlesTexture.Height / 2f);
            _vOblongsOrigin = new Vector2(_vOblongsTexture.Width / 2f, _vOblongsTexture.Height / 2f);
            _hOblongsOrigin = new Vector2(_hOblongsTexture.Width / 2f, _hOblongsTexture.Height / 2f);

            _border = new Border(ScreenManager.ScreenWidth, ScreenManager.ScreenHeight, 25, ScreenManager.ScreenCenter);
            _border.Load(ScreenManager.GraphicsDevice, PhysicsSimulator);
            
            _lineBrush.Load(ScreenManager.GraphicsDevice);

            _obstacles = new BodyList();
            _obstaclesg = new GeomList();
            
            //@kester - motor & shaft body/geom list
            _wheelBodyList = new BodyList();
            _wheelGeomList = new GeomList();
            _motorCWBodyList = new BodyList();
            _motorCWGeomList = new GeomList();
            _motorACWBodyList = new BodyList();
            _motorACWGeomList = new GeomList();
            _shaftBodyList = new BodyList();
            _shaftGeomList = new GeomList();
            //_debugSW = File.CreateText("debug.txt"); // debug

            base.LoadContent();

            shape = 1;
            stat = false;
        }
        #endregion        

        #region Draw Method
        public override void Draw(GameTime gameTime)
        {


      
            ScreenManager.SpriteBatch.Begin(SpriteBlendMode.AlphaBlend);

            // testing the camera

            // 1) The camera will be moved by this offset.
            Vector2 offset= new Vector2(300, 300);

            // 2) This offset is added to the original position.
            Vector2 originalPosition = new Vector2 (100,100);

            // 3) Do this to EVERY SINGLE SPRITE in the game?!
            ScreenManager.SpriteBatch.Draw(_circlesTexture, originalPosition + offset, Color.Wheat);

            

            // ------------------

            #region Draw Texts            
            // Draw texts
            if (_currScreenMode == (int) ScreenMode.LEditor)
            {
                if (placeMode == 1)
                    str = "Free to move";
                if (placeMode == 2)
                    str = "Static";
                if (placeMode == 3)
                    str = "Activatable static";
                _strScreenMode = "1 - Level Editor Mode";
            }
            else if (_currScreenMode == (int) ScreenMode.VEditor)
            {
                if (shape == 1)
                    str = "Wheel";
                if (shape == 2)
                    str = "Motor CW";
                if (shape == 3)
                    str = "Motor Anti-CW";
                if (shape == 4)
                    str = "Joint";
                _strScreenMode = "2 - Vehicle Editor Mode";
            }
            else if (_currScreenMode == (int) ScreenMode.Game)
            {
                _strScreenMode = "3 - Game Mode";
                str = "";
            }
            ScreenManager.SpriteBatch.DrawString(ScreenManager.SpriteFonts.DiagnosticSpriteFont,
                                     str,
                                     new Vector2(875, 200), Color.White);
            ScreenManager.SpriteBatch.DrawString(ScreenManager.SpriteFonts.MenuSpriteFont,
                                     _strScreenMode,
                                     new Vector2(50, 100), Color.White);

            ScreenManager.SpriteBatch.DrawString(ScreenManager.SpriteFonts.DiagnosticSpriteFont,
                                     triangleCount.ToString(),
                                     new Vector2(875, 400), Color.White);

            


            // firstClick.ToString()
            // if(vertices!=null)
            // vertices.Count.ToString()
            
            /*
            ScreenManager.SpriteBatch.DrawString(ScreenManager.SpriteFonts.DiagnosticSpriteFont,
                                     vertices3Counter.ToString(),
                                     new Vector2(775, 500), Color.White);

            ScreenManager.SpriteBatch.DrawString(ScreenManager.SpriteFonts.DiagnosticSpriteFont,
                                     vertices3[0].Position.ToString(),
                                     new Vector2(575, 300), Color.White);

            ScreenManager.SpriteBatch.DrawString(ScreenManager.SpriteFonts.DiagnosticSpriteFont,
                                     vertices3[1].Position.ToString(),
                                     new Vector2(575, 400), Color.White);

            ScreenManager.SpriteBatch.DrawString(ScreenManager.SpriteFonts.DiagnosticSpriteFont,
                                     vertices3[2].Position.ToString(),
                                     new Vector2(575, 500), Color.White);

            ScreenManager.SpriteBatch.DrawString(ScreenManager.SpriteFonts.DiagnosticSpriteFont,
                                     vertices3[3].Position.ToString(),
                                     new Vector2(375, 300), Color.White);

            ScreenManager.SpriteBatch.DrawString(ScreenManager.SpriteFonts.DiagnosticSpriteFont,
                                     vertices3[4].Position.ToString(),
                                     new Vector2(375, 400), Color.White);

            ScreenManager.SpriteBatch.DrawString(ScreenManager.SpriteFonts.DiagnosticSpriteFont,
                                     vertices3[5].Position.ToString(),
                                     new Vector2(375, 500), Color.White); 
             */

            ScreenManager.SpriteBatch.DrawString(ScreenManager.SpriteFonts.DiagnosticSpriteFont,
                                     point.ToString(),
                                     new Vector2(775, 600), Color.White);

            ScreenManager.SpriteBatch.DrawString(ScreenManager.SpriteFonts.DiagnosticSpriteFont,
                                     pointCheck.ToString(),
                                     new Vector2(775, 700), Color.White);
             

            #endregion

            #region Draw Objects

            #region Drawing Objects for level editor
            //@drawing for level editor
            if (_currScreenMode != (int) ScreenMode.VEditor)
            {
                foreach (Body b in _obstacles)
                {
                    //@daniel -  draw shapes
                    if (b.shape == 1)
                        ScreenManager.SpriteBatch.Draw(_squaresTexture, b.Position, null, Color.White, b.Rotation, _squaresOrigin, 1, SpriteEffects.None, 1);
                    if (b.shape == 2)
                        ScreenManager.SpriteBatch.Draw(_circlesTexture, b.Position, null, Color.White, b.Rotation, _circlesOrigin, 1, SpriteEffects.None, 1);
                    if (b.shape == 3)
                        ScreenManager.SpriteBatch.Draw(_vOblongsTexture, b.Position, null, Color.White, b.Rotation, _vOblongsOrigin, 1, SpriteEffects.None, 1);
                    if (b.shape == 4)
                        ScreenManager.SpriteBatch.Draw(_hOblongsTexture, b.Position, null, Color.White, b.Rotation, _hOblongsOrigin, 1, SpriteEffects.None, 1);
                    if (b.shape == 5)
                        ScreenManager.SpriteBatch.Draw(_polygonTexture, b.Position, null, Color.White, b.Rotation, _polygonOrigin, 1, SpriteEffects.None, 1);
                    if (b.shape == 6)
                    {
                        //ScreenManager.SpriteBatch.Draw(_circlesTexture, b.Position, null, Color.White, b.Rotation, _circlesOrigin, 1, SpriteEffects.None, 1);
                    }
                }

                // draw lines as vertices are being drawn
                if ( vertices!=null )
                foreach (Vector2 v in vertices)
                {
                    
                    for (int i = 0; i < vertices.Count - 1; i++)
                    {
                        _lineBrush.Draw(ScreenManager.SpriteBatch, vertices[i], vertices[i + 1]);
                    }

                    _lineBrush.Draw(ScreenManager.SpriteBatch, vertices[vertices.Count - 1], vertices[0]); // tie off the end
                }

                
                // draw shape
                foreach (Geom g in _obstaclesg)
                {
                    if (g.Body.shape == 6)
                    {

                        Vertices v = g.WorldVertices;

                        for (int i = 0; i < v.Count - 1; i++)
                        {
                            _lineBrush.Draw(ScreenManager.SpriteBatch, v[i], v[i + 1]);
                            
                        }

                        _lineBrush.Draw(ScreenManager.SpriteBatch, v[v.Count-1], v[0]); // tie off the end


                        // testing fill following of object
                        
                        Vector2 normVect = normalizeScnCoord(v[0]);
                        
                        vertices3[g.Body.triangles[0] * 3 + 0].Position = new Vector3(normVect, 0);

                        normVect = normalizeScnCoord(v[1]);

                        vertices3[g.Body.triangles[0] * 3 + 1].Position = new Vector3(normVect, 0);

                        normVect = normalizeScnCoord(v[2]);

                        vertices3[g.Body.triangles[0] * 3 + 2].Position = new Vector3(normVect, 0);

                        

                    }
                }
                
                //@daniel - draw item icons   
                if (_currScreenMode != (int)ScreenMode.Game)
                {
                    if (shape == 1)
                        ScreenManager.SpriteBatch.Draw(_squaresTexture, itemPos, null, Color.White, 0, _squaresOrigin, 1, SpriteEffects.None, 1);
                    if (shape == 2)
                        ScreenManager.SpriteBatch.Draw(_circlesTexture, itemPos, null, Color.White, 0, _circlesOrigin, 1, SpriteEffects.None, 1);
                    if (shape == 3)
                        ScreenManager.SpriteBatch.Draw(_vOblongsTexture, itemPos, null, Color.White, 0, _vOblongsOrigin, 1, SpriteEffects.None, 1);
                    if (shape == 4)
                        ScreenManager.SpriteBatch.Draw(_hOblongsTexture, itemPos, null, Color.White, 0, _hOblongsOrigin, 1, SpriteEffects.None, 1);
                    if (shape == 5)
                        ScreenManager.SpriteBatch.Draw(_polygonTexture, itemPos, null, Color.White, 0, _polygonOrigin, 1, SpriteEffects.None, 1);
                    if (shape == 6)
                    {
                        ScreenManager.SpriteBatch.DrawString(ScreenManager.SpriteFonts.MenuSpriteFont,
                                     "Dynamic object",
                                     itemPos-new Vector2(100, 0), Color.White);
                    }
                }
            }
            #endregion

            #region Drawing Objects for Vehicle Editor
            //@kester 
            if (_currScreenMode != (int) ScreenMode.LEditor)
            {
                // draw wheel
                foreach (Body b in _wheelBodyList)
                {                   
                    ScreenManager.SpriteBatch.Draw(_wheelTexture, b.Position, null, Color.White, b.Rotation, _wheelOrigin, 1, SpriteEffects.None, 1);
                }
                // draw cw motor
                foreach (Body b in _motorCWBodyList)
                {
                    ScreenManager.SpriteBatch.Draw(_motorCWTexture, b.Position, null, Color.White, b.Rotation, _wheelOrigin, 1, SpriteEffects.None, 1);
                }
                 // draw acw motor
                foreach (Body b in _motorACWBodyList)
                {
                    ScreenManager.SpriteBatch.Draw(_motorACWTexture, b.Position, null, Color.White, b.Rotation, _wheelOrigin, 1, SpriteEffects.None, 1);
                }
                // draw shafts 
                foreach (GeomPair gp in _shaftGeomPairList)
                {
                    ScreenManager.SpriteBatch.Draw(_shaftTexture,
                        new Rectangle((int)gp.getOriginPosition().X, (int)gp.getOriginPosition().Y,
                            (int)gp.getDifference().Length(), 20),
                       null, Color.Green, gp.getRotation(), _shaftOrigin,
                       SpriteEffects.None, 0.0f);
                }
                if (_currScreenMode != (int)ScreenMode.Game) // draw icons
                {
                    if (shape == 1)// 1 for Clockwise wheel
                        ScreenManager.SpriteBatch.Draw(_wheelTexture, itemPos, null, Color.White, 0, _wheelOrigin, 1, SpriteEffects.None, 1);
                    if (shape == 2)// 2 for Anti-Clockwise wheel
                        ScreenManager.SpriteBatch.Draw(_motorCWTexture, itemPos, null, Color.White, 0, _wheelOrigin, 1, SpriteEffects.None, 1);
                    if (shape == 3)// 3 for Clockwise wheel
                        ScreenManager.SpriteBatch.Draw(_motorACWTexture, itemPos, null, Color.White, 0, _wheelOrigin, 1, SpriteEffects.None, 1);
                    if (shape == 4)// 4 for draw joints
                        ScreenManager.SpriteBatch.Draw(_shaftTextureIcon, itemPos, null, Color.White, 0, _shaftOriginIcon, 1, SpriteEffects.None, 1);
                }
            }
            #endregion
            
            _border.Draw(ScreenManager.SpriteBatch);

            // Temp draw mouse spring
            if (_mousePickSpring != null)
            {
                _lineBrush.Draw(ScreenManager.SpriteBatch,
                                _mousePickSpring.Body.GetWorldPosition(_mousePickSpring.BodyAttachPoint),
                                _mousePickSpring.WorldAttachPoint);
            }           

            ScreenManager.SpriteBatch.End();
            #endregion

            #region xbox controls - cursors
            // @jack - cursor updates to handle Xbox gamepad cursor control
            deltaMovement = GamePad.GetState(PlayerIndex.One).ThumbSticks.Left;
            deltaMovement.Y *= -1;

            #if !XBOX360
            //use the mouse position as the cursor position
            MouseState mouseState = Mouse.GetState();
            position.X = mouseState.X;
            position.Y = mouseState.Y;
            #endif

            // modify position using delta, the CursorSpeed, and
            // the elapsed game time.
            position += deltaMovement * CursorSpeed *
                (float)gameTime.ElapsedGameTime.TotalSeconds;

            #if XBOX360
            // clamp the cursor position to the viewport, so that it can't move off the
            // screen.
            Viewport vp = GraphicsDevice.Viewport;
            position.X = MathHelper.Clamp(position.X, vp.X, vp.X + vp.Width);
            position.Y = MathHelper.Clamp(position.Y, vp.Y, vp.Y + vp.Height);
            #else
            // set the new mouse position using the combination of mouse and gamepad data.
            Mouse.SetPosition((int)position.X, (int)position.Y);    
            #endif            
            deltaMovement = GamePad.GetState(PlayerIndex.One).ThumbSticks.Left;
            deltaMovement.Y *= -1;

            // end cursor updates ------------------------------------------------------
            #endregion
            
            #region TestFill

            basicEffect.Begin();
            basicEffect.Techniques[0].Passes[0].Begin();
            basicEffect.CommitChanges();

            // Triangle vertex based on center at (0,0,0)
            //Vector3 p = new Vector3(0, 0, 0);
            

            //Color black = new Color(new Vector3(0,0,0));
            //vertices3[3].Color = black;
            
            // Draw the triangle
            if (vertices3Counter > 2)
            {
                ScreenManager.GraphicsDevice.RenderState.CullMode = CullMode.None;
                ScreenManager.GraphicsDevice.DrawUserPrimitives(PrimitiveType.TriangleList, vertices3, 0, vertices3Counter / 3);
            }
            basicEffect.Techniques[0].Passes[0].End();
            basicEffect.End();

            #endregion

            base.Draw(gameTime);
        }
        #endregion        

        #region Math Functions

        public Vector2 normalizeScnCoord(Vector2 v2)
        {
            float x = (float)((v2.X - 512) / 512);
            float y = -(float)((v2.Y - 384) / 384);
            return new Vector2(x, y);
        }

        #endregion

        #region HandleInput Method
        public override void HandleInput(InputState input)
        {
            if (FirstRun)
            {
                ScreenManager.AddScreen(new PauseScreen(GetTitle(), GetDetails(), this));
                FirstRun = false;
            }
            if (input.PauseGame)
            {
                ScreenManager.AddScreen(new PauseScreen(GetTitle(), GetDetails(), this));
            }
            else
            {
                HandleMouseInput(input);
            }
            
            if (input.CurrentKeyboardState.IsKeyDown(Keys.S) && input.LastKeyboardState.IsKeyUp(Keys.S))
            {
                
                if (placeMode == 1) // free to move
                {
                    stat = true;
                    placeMode = 2;
                }
                else
                if (placeMode == 2) // static
                {
                    placeMode = 3;
                }
                else
                if (placeMode == 3) // activatable static
                {
                    stat = false;
                    placeMode = 1;
                }
            }
                

            // shape change
            if (input.CurrentKeyboardState.IsKeyDown(Keys.PageUp) && input.LastKeyboardState.IsKeyUp(Keys.PageUp))
            {
                if (vertices != null) vertices.Clear();
                if (shape < maxShapes)
                    shape++;
                else shape = 1;

                if (shape == 6)
                    firstClick = true;
            }
            else if (input.CurrentKeyboardState.IsKeyDown(Keys.PageDown) && input.LastKeyboardState.IsKeyUp(Keys.PageDown))
            {
                if( vertices!=null ) vertices.Clear();
                if (shape > 1)
                    shape--;
                else shape = maxShapes;

                if (shape == 6)
                    firstClick = true;
            }

            //@kester - start or stop sim
            if (!input.LastKeyboardState.IsKeyDown(Keys.F2) && input.CurrentKeyboardState.IsKeyDown(Keys.F2))
            {
                _flagStartSim = !_flagStartSim;
                foreach (Body b in _motorCWBodyList)
                {
                    if (_flagStartSim) b.IsStatic = false;
                    else b.IsStatic = true;
                }
                foreach (Body b in _motorACWBodyList)
                {
                    if (_flagStartSim) b.IsStatic = false;
                    else b.IsStatic = true;
                }
                foreach (Body b in _wheelBodyList)
                {
                    if (_flagStartSim) b.IsStatic = false;
                    else b.IsStatic = true;
                }
            }
            //@kester - start or stop motor
            if (!input.LastKeyboardState.IsKeyDown(Keys.F3) && input.CurrentKeyboardState.IsKeyDown(Keys.F3))
            {
                _motorDirection = !_motorDirection;
            }
            //@kester - save level/ editor
            if (!input.LastKeyboardState.IsKeyDown(Keys.F4) && input.CurrentKeyboardState.IsKeyDown(Keys.F4))
            {
                string _currStr;
                if (File.Exists("temp.txt")) File.Delete("temp.txt");

                if (_currScreenMode == (int)ScreenMode.LEditor) // save level
                {                    
                    File.Copy("levels.txt", "temp.txt");// copy to temp file for counting purpose
                    _levelCountSR = File.OpenText("temp.txt");
                    _currStr = _levelCountSR.ReadLine();
                    while (_currStr != null)
                    {
                        if (_currStr.StartsWith("//Map")) // load map no.
                        {
                            _currStr = _levelCountSR.ReadLine();
                            ++_totalMapCount;
                        }
                        _currStr = _levelCountSR.ReadLine();
                    }
                    _levelCountSR.Close();//end counting

                    _levelSW = File.AppendText("levels.txt");
                    ++_totalMapCount;
                    saveToFile();
                }
                else if (_currScreenMode == (int)ScreenMode.VEditor)  // save vehicle
                {
                    File.Copy("vehicles.txt", "temp.txt");// copy to temp file for counting purpose
                    _vehicleCountSR = File.OpenText("temp.txt");
                    _currStr = _vehicleCountSR.ReadLine();
                    while (_currStr != null)
                    {
                        if (_currStr.StartsWith("//Vehicle")) // load vehicle no.
                        {
                            _currStr = _vehicleCountSR.ReadLine();
                            ++_totalVehicleCount;
                        }
                        _currStr = _vehicleCountSR.ReadLine();
                    }
                    _vehicleCountSR.Close();//end counting

                    _vehicleSW = File.AppendText("vehicles.txt");
                    ++_totalVehicleCount;
                    saveToFile();
                }
            }
            //@kester - save level/ editor
            if (!input.LastKeyboardState.IsKeyDown(Keys.F5) && input.CurrentKeyboardState.IsKeyDown(Keys.F5))
            {
                //load files 
                if (_currScreenMode == (int)ScreenMode.LEditor)
                {
                    _levelSR = File.OpenText("levels.txt");
                }
                else if (_currScreenMode == (int)ScreenMode.VEditor)
                {
                    _vehicleSR = File.OpenText("vehicles.txt");
                }
                    
                //_debugSW = File.CreateText("debug.txt");
                loadFromFile();
            }
            //@kester - switch screen mode
            // 1 - Level Editor
            // 2 - Vehicle Editor
            // 3 - Game
            if (!input.LastKeyboardState.IsKeyDown(Keys.D1) && input.CurrentKeyboardState.IsKeyDown(Keys.D1))
            {
                _currScreenMode = (int) ScreenMode.LEditor;
                maxShapes = 5;
                shape = 1;
                stat = false;
                placeMode = 1;
                //str = "Free to move";
            }else if (!input.LastKeyboardState.IsKeyDown(Keys.D2) && input.CurrentKeyboardState.IsKeyDown(Keys.D2))
            {
                _currScreenMode = (int) ScreenMode.VEditor;
                maxShapes = 4;
                shape = 1;
                placeMode = 1;
                stat = true;
            }else if (!input.LastKeyboardState.IsKeyDown(Keys.D3) && input.CurrentKeyboardState.IsKeyDown(Keys.D3))
            {
                _currScreenMode = (int) ScreenMode.Game;
            }

            #region xbox controls
            /////////////////////////////////////////////////////////////////////////////////////////////////////////
            //                                                                                                     //
            //                                        Gamepad Controls                                             //
            //                                                                                                     //
            /////////////////////////////////////////////////////////////////////////////////////////////////////////

            // Gamepad control button B to change static and free-moving object
            if (input.CurrentGamePadState.IsButtonDown(Buttons.B) && input.LastGamePadState.IsButtonUp(Buttons.B))
                stat = !stat;
            // --------------------


            // Gamepad analog controls to move left and right
            /*
            if (input.CurrentGamePadState.IsButtonDown(Buttons.LeftThumbstickLeft))
            {
                _torque = -5000;
            }
            if (input.CurrentGamePadState.IsButtonDown(Buttons.LeftThumbstickRight))
            {
                _torque = 5000;
            }
             */
            // ---------------------
            
            if (!input.LastGamePadState.IsButtonDown(Buttons.LeftTrigger) && input.CurrentGamePadState.IsButtonDown(Buttons.LeftTrigger))
            {
                _flagStartSim = !_flagStartSim;
                
            }
            if (!input.LastGamePadState.IsButtonDown(Buttons.RightTrigger) && input.CurrentGamePadState.IsButtonDown(Buttons.RightTrigger))
            {
                _motorDirection = !_motorDirection;
            }

            // Gamepad right analog controls up and down to cycle through shapes
            if (input.CurrentGamePadState.IsButtonDown(Buttons.RightThumbstickUp) &&
               input.LastGamePadState.IsButtonUp(Buttons.RightThumbstickUp))
            {
                if (shape < maxShapes)
                    shape++;
                else shape = 1;
            }

            else if (input.CurrentGamePadState.IsButtonDown(Buttons.RightThumbstickDown)
                && input.LastGamePadState.IsButtonUp(Buttons.RightThumbstickDown))
            {
                if (shape > 1)
                    shape--;
                else shape = maxShapes;
            }
            // -----------------------------

            // Gamepad control button X to remove editor objects
            if (input.CurrentGamePadState.IsButtonDown(Buttons.X) && input.LastGamePadState.IsButtonUp(Buttons.X))
            {
                _pickedGeom = PhysicsSimulator.Collide(point);
                if (_pickedGeom != null)
                {
                    _obstacles.Remove(_pickedGeom.Body);
                    _pickedGeom.Body.Dispose();
                }
            }
            // ------------------------------


            //Gamepad control button A to add editor objects

            //@kester - joint mode
            if (shape == 7)
            {
                _flagDoJoint = true;
            }
            if (shape != 7)
            {
                _flagDoJoint = false;
            }
            // end

            // Dragging with the A button
            if (input.LastGamePadState.IsButtonDown(Buttons.A) &&
                input.CurrentGamePadState.IsButtonDown(Buttons.A))
            {
                if (dragged != null)
                {
                    if (dragged.IsStatic && dragged.shape >= 1 && dragged.shape <= maxShapes)
                    {
                        dragged.Position = new Vector2(position.X, position.Y);
                    }
                }
            }
            // -------------------------------

            // Releasing A button
            if (input.LastGamePadState.IsButtonDown(Buttons.A) &&
                input.CurrentGamePadState.IsButtonUp(Buttons.A))
            {
                dragged = null;
            }
            // --------------------------------            

            // Selecting objects
            if (input.LastGamePadState.IsButtonUp(Buttons.A) &&
                input.CurrentGamePadState.IsButtonDown(Buttons.A) &&
                !_flagDoJoint)
            {
                //create mouse spring
                _pickedGeom = PhysicsSimulator.Collide(position);
                if (_pickedGeom != null)
                {
                    if (!_flagDoJoint)
                    {
                        _mousePickSpring = SpringFactory.Instance.CreateFixedLinearSpring(PhysicsSimulator,
                                                                                          _pickedGeom.Body,
                                                                                          _pickedGeom.Body.
                                                                                              GetLocalPosition(position),
                                                                                          position, 150, 10);
                    }

                    // if static shape
                    if (_pickedGeom.Body.IsStatic && _pickedGeom.Body.shape >= 1 && _pickedGeom.Body.shape <= maxShapes)
                    {
                        dragged = _pickedGeom.Body;
                        //_pickedGeom.Body.Position = new Vector2(input.CurrentMouseState.X, input.CurrentMouseState.Y);
                    }

                    // if not static
                    // @kester spider web bug
                    /*
                    if (!_pickedGeom.Body.IsStatic)
                        _mousePickSpring = SpringFactory.Instance.CreateFixedLinearSpring(PhysicsSimulator,
                                                                                          _pickedGeom.Body,
                                                                                          _pickedGeom.Body.
                                                                                              GetLocalPosition(position),
                                                                                          position, 150, 10);
                    */
                }
                else
                {
                    // shapes
                    if (shape == 1 && _currScreenMode == (int) ScreenMode.LEditor)
                    {
                        _obstacles.Add(BodyFactory.Instance.CreateRectangleBody(PhysicsSimulator, 40, 40, 25));
                        _obstaclesg.Add(GeomFactory.Instance.CreateRectangleGeom(PhysicsSimulator, _obstacles[_obstacles.Count - 1], 40, 40));
                    }
                    if (shape == 2 && _currScreenMode == (int)ScreenMode.LEditor)
                    {
                        _obstacles.Add(BodyFactory.Instance.CreateCircleBody(PhysicsSimulator, 20, 30));
                        _obstaclesg.Add(GeomFactory.Instance.CreateCircleGeom(PhysicsSimulator, _obstacles[_obstacles.Count - 1], 20, 30));
                    }
                    if (shape == 3 && _currScreenMode == (int)ScreenMode.LEditor)
                    {
                        _obstacles.Add(BodyFactory.Instance.CreateRectangleBody(PhysicsSimulator, 20, 80, 30));
                        _obstaclesg.Add(GeomFactory.Instance.CreateRectangleGeom(PhysicsSimulator, _obstacles[_obstacles.Count - 1], 20, 80));
                    }
                    if (shape == 4 && _currScreenMode == (int)ScreenMode.LEditor)
                    {
                        _obstacles.Add(BodyFactory.Instance.CreateRectangleBody(PhysicsSimulator, 80, 20, 30));
                        _obstaclesg.Add(GeomFactory.Instance.CreateRectangleGeom(PhysicsSimulator, _obstacles[_obstacles.Count - 1], 80, 20));
                    }
                    if (shape == 5 && _currScreenMode == (int)ScreenMode.LEditor)
                    {
                        _obstacles.Add(BodyFactory.Instance.CreatePolygonBody(PhysicsSimulator, verts, 5));
                        _obstaclesg.Add(GeomFactory.Instance.CreatePolygonGeom(PhysicsSimulator, _obstacles[_obstacles.Count - 1], verts, 0));
                    }
                    if (shape == 1 && _currScreenMode == (int)ScreenMode.VEditor)
                    {
                        _wheelBodyList.Add(BodyFactory.Instance.CreateCircleBody(PhysicsSimulator, 20, 5));
                        _wheelGeomList.Add(GeomFactory.Instance.CreateCircleGeom(PhysicsSimulator, _wheelBodyList[_wheelBodyList.Count - 1], 20, 30));

                    }
                    if ( _currScreenMode == (int) ScreenMode.LEditor)
                    {
                        _obstacles[_obstacles.Count - 1].IsStatic = stat;
                        _obstacles[_obstacles.Count - 1].Position = new Vector2(input.CurrentMouseState.X, input.CurrentMouseState.Y);
                        _obstacles[_obstacles.Count - 1].shape = shape;
                        _obstaclesg[_obstaclesg.Count - 1].FrictionCoefficient = 1.0f;
                        if (stat)
                            dragged = _obstacles[_obstacles.Count - 1];
                    }
                   else 
                   {
                        _wheelBodyList[_wheelBodyList.Count - 1].IsStatic = false;
                        _wheelBodyList[_wheelBodyList.Count - 1].Position = new Vector2(input.CurrentMouseState.X, input.CurrentMouseState.Y);
                        _wheelBodyList[_wheelBodyList.Count - 1].shape = shape;
                        _wheelGeomList[_wheelGeomList.Count - 1].FrictionCoefficient = 1.0f;
                   }
                }
            }
            // finishing joints
            else if (input.LastGamePadState.IsButtonDown(Buttons.A) &&
                     input.CurrentGamePadState.IsButtonUp(Buttons.A) &&
                    !_flagDoJoint)
            {
                //destroy mouse spring
                if (_mousePickSpring != null && _mousePickSpring.IsDisposed == false)
                {
                    _mousePickSpring.Dispose();
                    _mousePickSpring = null;
                }
            }

            //move anchor point
            if (input.CurrentGamePadState.IsButtonUp(Buttons.A) && _mousePickSpring != null)
            {
                _mousePickSpring.WorldAttachPoint = point;
            }

            if (input.LastGamePadState.IsButtonUp(Buttons.A) &&
               input.CurrentGamePadState.IsButtonDown(Buttons.A) &&
               _flagDoJoint)
            {
                _pickedGeom = PhysicsSimulator.Collide(point);
                _selectedGeoms.Add(_pickedGeom);
            }
            else if (input.LastGamePadState.IsButtonDown(Buttons.A) &&
                     input.CurrentGamePadState.IsButtonUp(Buttons.A) &&
                _flagDoJoint)
            {
                _pickedGeom2 = PhysicsSimulator.Collide(point);
                if (_pickedGeom2 != null)
                {
                    _selectedGeoms.Add(_pickedGeom2);

                    // calculates hypothenous to get the width of shaft
                    _shaftWidth = (float)Math.Sqrt(Math.Pow(Math.Abs(_pickedGeom.Position.X - _pickedGeom2.Position.X), 2) +
                        Math.Pow(Math.Abs(_pickedGeom.Position.Y - _pickedGeom2.Position.Y), 2));
                       
                    // create temp shaft body
                    Body _currBody = BodyFactory.Instance.CreateRectangleBody(PhysicsSimulator, _shaftWidth, 10, 5);

                    // do some calculation to get the correct orientation
                    Vector2 endPoint = _pickedGeom2.Position;
                    Vector2 startPoint = _pickedGeom.Position;
                    Vector2.Subtract(ref endPoint, ref startPoint, out _shaftDifference);
                    CalculateRotation(_shaftDifference);
                    _currBody.Rotation = _shaftRotation;
                    _currBody.Position = Vector2.Add(_pickedGeom.Position,
                        Vector2.Multiply(Vector2.Subtract(_pickedGeom2.Position, _pickedGeom.Position), 0.5f));

                    //add body of shaft to list
                    _shaftBodyList.Add(_currBody);

                    // now adding joints ...
                    _jointRev = JointFactory.Instance.CreateRevoluteJoint(PhysicsSimulator, _pickedGeom.Body, _currBody, _pickedGeom.Body.Position);
                    _jointRevList.Add(_jointRev);
                    _jointRev = JointFactory.Instance.CreateRevoluteJoint(PhysicsSimulator, _currBody, _pickedGeom2.Body, _pickedGeom2.Body.Position);
                    _jointRevList.Add(_jointRev);

                    // set geom of shaft same collision group as motor wheel so they wont collide 
                    Geom _currGeom = GeomFactory.Instance.CreateRectangleGeom(PhysicsSimulator, _currBody, _shaftWidth, 10);
                    _currGeom.CollisionGroup = 1;
                    _currGeom.SetBody(_currBody);
                    _shaftGeomList.Add(_currGeom);
                    
                    // pair up the 2x selected geom and add to the GeomPair list
                    GeomPair gp = new GeomPair(_pickedGeom, _pickedGeom2);
                    gp.setDifference(_shaftDifference);
                    gp.setRotation(_shaftRotation);
                    gp.setOrigin(new Vector2(_shaftWidth / 2, 5));
                    _shaftGeomPairList.Add(gp);                    
                }
            }
            // -------------------------------
            #endregion

            base.HandleInput(input);
        }
        #endregion

        #region HandleMouseInput Method
        private void HandleMouseInput(InputState input)
        {
            // mouse position 
            point = new Vector2(input.CurrentMouseState.X, input.CurrentMouseState.Y);

            //@kester - joint mode
            if (shape == 4 && _currScreenMode == (int)ScreenMode.VEditor) _flagDoJoint = true;
            else _flagDoJoint = false;           

            //removing objects
            if (input.LastMouseState.RightButton == ButtonState.Released &&
                input.CurrentMouseState.RightButton == ButtonState.Pressed)
            {
                _pickedGeom = PhysicsSimulator.Collide(point);
                if (_pickedGeom != null)
                {
                    _obstacles.Remove(_pickedGeom.Body);
                    _obstaclesg.Remove(_pickedGeom);
                    _motorCWBodyList.Remove(_pickedGeom.Body);
                    _motorCWGeomList.Remove(_pickedGeom);
                    _motorACWBodyList.Remove(_pickedGeom.Body);
                    _motorACWGeomList.Remove(_pickedGeom);
                    _wheelBodyList.Remove(_pickedGeom.Body);
                    _wheelGeomList.Remove(_pickedGeom);
                    _shaftBodyList.Remove(_pickedGeom.Body);
                    _shaftGeomList.Remove(_pickedGeom);
                    _pickedGeom.Body.Dispose();
                }

            }

            // if dragging static
            if (input.LastMouseState.LeftButton == ButtonState.Pressed &&
                input.CurrentMouseState.LeftButton == ButtonState.Pressed )
            {
                if (dragged != null)
                {
                    if (dragged.IsStatic && dragged.shape >= 1 && dragged.shape <= maxShapes && _currScreenMode == (int)ScreenMode.LEditor)
                    {
                        dragged.Position = new Vector2(input.CurrentMouseState.X, input.CurrentMouseState.Y);
                    }
                }
            }

            if (input.LastMouseState.LeftButton == ButtonState.Pressed &&
                input.CurrentMouseState.LeftButton == ButtonState.Released)
            {
                dragged = null;
            }

            // on left click
            if (input.LastMouseState.LeftButton == ButtonState.Released &&
                input.CurrentMouseState.LeftButton == ButtonState.Pressed &&
                !_flagDoJoint)
            {
                //create mouse spring
                _pickedGeom = PhysicsSimulator.Collide(point);
                if (_pickedGeom != null)
                {

                    //@kester
                    if (!_flagDoJoint)
                    {
                        _mousePickSpring = SpringFactory.Instance.CreateFixedLinearSpring(PhysicsSimulator,
                                                                                          _pickedGeom.Body,
                                                                                          _pickedGeom.Body.
                                                                                              GetLocalPosition(point),
                                                                                          point, 150, 10);
                    }
                
                    // if static shape
                    if (_pickedGeom.Body.IsStatic && _pickedGeom.Body.shape >= 1 && _pickedGeom.Body.shape <= maxShapes)
                    {
                        dragged = _pickedGeom.Body;
                        //_pickedGeom.Body.Position = new Vector2(input.CurrentMouseState.X, input.CurrentMouseState.Y);
                    }

                    // if not static
                    //@kester - this part cause the spider web effect..
                    /*
                    if (!_pickedGeom.Body.IsStatic)
                        _mousePickSpring = SpringFactory.Instance.CreateFixedLinearSpring(PhysicsSimulator,
                                                                                          _pickedGeom.Body,
                                                                                          _pickedGeom.Body.
                                                                                              GetLocalPosition(point),
                                                                                          point, 150, 10);
                    */
                }
                else // not clicking on objects, creating shapes
                {
                    // shapes
                    if (_currScreenMode == (int)ScreenMode.LEditor)
                    {
                        if (shape == 1)
                        {
                            _obstacles.Add(BodyFactory.Instance.CreateRectangleBody(PhysicsSimulator, 40, 40, 25));
                            _obstaclesg.Add(GeomFactory.Instance.CreateRectangleGeom(PhysicsSimulator, _obstacles[_obstacles.Count - 1], 40, 40));
                        }

                        if (shape == 2)
                        {
                            _obstacles.Add(BodyFactory.Instance.CreateCircleBody(PhysicsSimulator, 20, 30));
                            _obstaclesg.Add(GeomFactory.Instance.CreateCircleGeom(PhysicsSimulator, _obstacles[_obstacles.Count - 1], 20, 30));
                        }

                        if (shape == 3)
                        {
                            _obstacles.Add(BodyFactory.Instance.CreateRectangleBody(PhysicsSimulator, 20, 80, 30));
                            _obstaclesg.Add(GeomFactory.Instance.CreateRectangleGeom(PhysicsSimulator, _obstacles[_obstacles.Count - 1], 20, 80));
                        }

                        if (shape == 4)
                        {
                            _obstacles.Add(BodyFactory.Instance.CreateRectangleBody(PhysicsSimulator, 80, 20, 30));
                            _obstaclesg.Add(GeomFactory.Instance.CreateRectangleGeom(PhysicsSimulator, _obstacles[_obstacles.Count - 1], 80, 20));
                        }

                        if (shape == 5)
                        {
                            _obstacles.Add(BodyFactory.Instance.CreatePolygonBody(PhysicsSimulator, verts, 5));
                            _obstaclesg.Add(GeomFactory.Instance.CreatePolygonGeom(PhysicsSimulator, _obstacles[_obstacles.Count - 1], verts, 0));
                        }

                        if (shape == 6)
                        {
                            if (firstClick == true)
                            {
                                vertices = new Vertices();
                                firstClick = false;
                            }
                            
                            vertices.Add(point);

                            Vector2 normVect = normalizeScnCoord(point);
                            vertices3[vertices3Counter++].Position = new Vector3(normVect, 0);

                        }

                    }

                    //@kester - add wheels
                    if (_currScreenMode == (int)ScreenMode.VEditor)
                    {                            
                        Body _currBody = BodyFactory.Instance.CreateCircleBody(PhysicsSimulator, 34, 10);
                        _currBody.IsStatic = true;

                        //_currBody.SetIndex(_wheelBodyList.Count + _motorCWBodyList.Count + _motorACWBodyList.Count);

                        if (shape == 1) // wheel list
                        {
                            _wheelBodyList.Add(_currBody);
                            Geom _currGeom = GeomFactory.Instance.CreateCircleGeom(PhysicsSimulator, _wheelBodyList[_wheelBodyList.Count - 1], 34, 34);
                            _currGeom.CollisionGroup = 1; 
                            _currGeom.SetIndex(_wheelBodyList.Count + _motorCWBodyList.Count + _motorACWBodyList.Count);
                            _wheelGeomList.Add(_currGeom);
                        }
                        else if (shape == 2) // cw wheel list
                        {
                            _motorCWBodyList.Add(_currBody);
                            Geom _currGeom = GeomFactory.Instance.CreateCircleGeom(PhysicsSimulator, _motorCWBodyList[_motorCWBodyList.Count - 1], 34, 34);
                            _currGeom.CollisionGroup = 1; 
                            _currGeom.SetIndex(_wheelBodyList.Count + _motorCWBodyList.Count + _motorACWBodyList.Count);
                            _motorCWGeomList.Add(_currGeom);
                        }else if (shape == 3) // acw wheel list
                        {
                            _motorACWBodyList.Add(_currBody);
                            Geom _currGeom = GeomFactory.Instance.CreateCircleGeom(PhysicsSimulator, _motorACWBodyList[_motorACWBodyList.Count - 1], 34, 34);
                            _currGeom.CollisionGroup = 1;                            
                            _motorACWGeomList.Add(_currGeom);
                            _currGeom.SetIndex(_wheelBodyList.Count + _motorCWBodyList.Count + _motorACWBodyList.Count);
                            _motorACWGeomList.Add(_currGeom);
                        }  
                     
                    }

                    if (_currScreenMode == (int)ScreenMode.LEditor && shape!= 6)    // if not adding dynamic
                    {
                        _obstacles[_obstacles.Count - 1].IsStatic = stat;
                        _obstacles[_obstacles.Count - 1].Position = new Vector2(input.CurrentMouseState.X, input.CurrentMouseState.Y);
                        _obstacles[_obstacles.Count - 1].shape = shape;
                        _obstaclesg[_obstaclesg.Count - 1].FrictionCoefficient = 1.0f;
                        
                        if (placeMode == 3)
                        {
                            _obstacles[_obstacles.Count - 1].activatable = true;
                            _obstaclesg[_obstacles.Count - 1].OnCollision += OnCollision;
                            PhysicsSimulator.BroadPhaseCollider.OnBroadPhaseCollision += OnBroadPhaseCollision;
                        }

                        if (stat)
                            dragged = _obstacles[_obstacles.Count - 1];
                    }
                    else if (_currScreenMode == (int)ScreenMode.VEditor)
                    {
                        if (shape == 1)
                        {
                            _wheelBodyList[_wheelBodyList.Count - 1].IsStatic = stat;
                            _wheelBodyList[_wheelBodyList.Count - 1].Position = new Vector2(input.CurrentMouseState.X, input.CurrentMouseState.Y);
                            _wheelBodyList[_wheelBodyList.Count - 1].shape = shape;
                            _wheelGeomList[_wheelGeomList.Count - 1].FrictionCoefficient = 1.0f;
                        }
                        else if (shape == 2)
                        {
                            _motorCWBodyList[_motorCWBodyList.Count - 1].IsStatic = stat;
                            _motorCWBodyList[_motorCWBodyList.Count - 1].Position = new Vector2(input.CurrentMouseState.X, input.CurrentMouseState.Y);
                            _motorCWBodyList[_motorCWBodyList.Count - 1].shape = shape;
                            _motorCWGeomList[_motorCWGeomList.Count - 1].FrictionCoefficient = 1.0f;
                        }
                        else if (shape == 3)
                        {
                            _motorACWBodyList[_motorACWBodyList.Count - 1].IsStatic = stat;
                            _motorACWBodyList[_motorACWBodyList.Count - 1].Position = new Vector2(input.CurrentMouseState.X, input.CurrentMouseState.Y);
                            _motorACWBodyList[_motorACWBodyList.Count - 1].shape = shape;
                            _motorACWGeomList[_motorACWGeomList.Count - 1].FrictionCoefficient = 1.0f;
                        }
                    }
                    
                }
            } // end on left click
            else if (input.LastKeyboardState.IsKeyUp(Keys.A) &&
                input.CurrentKeyboardState.IsKeyDown(Keys.A) && shape == 6 && vertices!= null)
            {
                if (vertices.Count > 2)
                {
                    _obstacles.Add(BodyFactory.Instance.CreatePolygonBody(PhysicsSimulator, vertices, 100));
                    _obstaclesg.Add(GeomFactory.Instance.CreatePolygonGeom(PhysicsSimulator, _obstacles[_obstacles.Count - 1], vertices, 0));

                    _dynamicOrigin = vertices.GetCentroid();

                    _obstacles[_obstacles.Count - 1].IsStatic = stat;
                    _obstacles[_obstacles.Count - 1].shape = shape;
                    _obstaclesg[_obstaclesg.Count - 1].FrictionCoefficient = 1.0f;

                    // triangles
                    _obstacles[_obstacles.Count - 1].triangles[0] = triangleCount++;

                    if (placeMode == 3) // if activatable static
                    {
                        _obstacles[_obstacles.Count - 1].activatable = true;
                        _obstaclesg[_obstacles.Count - 1].OnCollision += OnCollision;
                        PhysicsSimulator.BroadPhaseCollider.OnBroadPhaseCollision += OnBroadPhaseCollision;
                    }

                    firstClick = true;

                    vertices.Clear();
                }
            }
            else if (input.LastMouseState.LeftButton == ButtonState.Pressed &&
                     input.CurrentMouseState.LeftButton == ButtonState.Released &&
                    !_flagDoJoint)
            {
                //destroy mouse spring
                if (_mousePickSpring != null && _mousePickSpring.IsDisposed == false)
                {
                    _mousePickSpring.Dispose();
                    _mousePickSpring = null;
                }
            }

            //move anchor point
            if (input.CurrentMouseState.LeftButton == ButtonState.Pressed && _mousePickSpring != null)
            {
                _mousePickSpring.WorldAttachPoint = point;
            }

            //@kester - link 2 geoms
            if (input.LastMouseState.LeftButton == ButtonState.Released &&
                input.CurrentMouseState.LeftButton == ButtonState.Pressed && _flagDoJoint)
            {
                _pickedGeom = PhysicsSimulator.Collide(point);                
                _selectedGeoms.Add(_pickedGeom);
            }
            else if (input.LastMouseState.LeftButton == ButtonState.Pressed &&
                     input.CurrentMouseState.LeftButton == ButtonState.Released 
                        && _flagDoJoint && _pickedGeom.Body.Enabled != false)
            {
                _pickedGeom2 = PhysicsSimulator.Collide(point);
                if (_pickedGeom2 != null && _pickedGeom2.Body.Enabled != false)
                {                    
                    _selectedGeoms.Add(_pickedGeom2);

                    // calculates hypothenous to get the length of shaft
                    float _shaftWidth = (float) Math.Sqrt(Math.Pow(Math.Abs(_pickedGeom.Position.X - _pickedGeom2.Position.X), 2) +
                        Math.Pow(Math.Abs(_pickedGeom.Position.Y - _pickedGeom2.Position.Y), 2));                                                           

                    // get id for the 2 geom
                    _geom1Id = _pickedGeom.GetIndex();
                    _geom2Id = _pickedGeom2.GetIndex();

                    // create temp shaft body
                    Body _currBody = BodyFactory.Instance.CreateRectangleBody(PhysicsSimulator, _shaftWidth, 15, 10);
                    
                    // do some calculation to get the correct orientation
                    Vector2 endPoint = _pickedGeom2.Position;
                    Vector2 startPoint = _pickedGeom.Position;
                    Vector2.Subtract(ref endPoint, ref startPoint, out _shaftDifference);
                    CalculateRotation(_shaftDifference); 
                    _currBody.Rotation = _shaftRotation;
                    _currBody.Position = Vector2.Add(_pickedGeom.Position,
                        Vector2.Multiply(Vector2.Subtract(_pickedGeom2.Position, _pickedGeom.Position), 0.5f));
                    
                    //add body of shaft to list
                    _shaftBodyList.Add(_currBody);
                    
                    // now adding joints ...
                    _jointRev = JointFactory.Instance.CreateRevoluteJoint(PhysicsSimulator, _pickedGeom.Body, _currBody, _pickedGeom.Body.Position);
                    _jointRevList.Add(_jointRev);
                    _jointRev = JointFactory.Instance.CreateRevoluteJoint(PhysicsSimulator, _currBody, _pickedGeom2.Body, _pickedGeom2.Body.Position);
                    _jointRevList.Add(_jointRev);

                    // set geom of shart same collision group as motor wheel so they wont collide 
                    Geom _currGeom = GeomFactory.Instance.CreateRectangleGeom(PhysicsSimulator, _currBody, _shaftWidth, 10);
                    _currGeom.CollisionGroup = 1;                   
                    _currGeom.SetBody(_currBody);
                    _currGeom.SetPairID(_geom1Id, _geom2Id);
                    _currGeom.SetWidth(_shaftWidth);
                    _currGeom.SetIndex(_shaftGeomList.Count + 1);
                    _currGeom.FrictionCoefficient = 1.0f;
                    //_currGeom.SetRotation(_shaftRotation);
                    _shaftGeomList.Add(_currGeom);

                    // pair up the 2x selected geom and add to the GeomPair list
                    GeomPair gp = new GeomPair(_pickedGeom, _pickedGeom2);
                    gp.setDifference(_shaftDifference);
                    gp.setRotation(_shaftRotation);
                    gp.setOrigin(new Vector2(_shaftWidth / 2, 5));
                    _shaftGeomPairList.Add(gp);
                    _shaftID = _shaftGeomPairList.Count;           
                }
            }
        }
        #endregion

        #region Update Method
        //@kester - update loop, update the following:
        // 1) motor's torque
        // 2) shaft body's rotation & position 
        // 3) enable/disable body for diff mode.
        public override void Update(GameTime gameTime, bool otherScreenHasFocus, bool coveredByOtherScreen)
        {
            //float x = (float)((point.X - 512) / 512);
            //float y = -(float)((point.Y - 384) / 384);
            Vector2 normVect = normalizeScnCoord(point);
            pointCheck = normVect;

            _torqueMotor = 35000.0f;

            // for changing direction
            float _torque = _torqueMotor;
            if (_motorDirection == true) _torque = _torqueMotor;
            else _torque = -1.0f * _torqueMotor;

            foreach (Body b in _motorCWBodyList)
            {
                if (_flagStartSim) b.ApplyTorque(_torque);
                else
                {
                    b.ApplyTorque(0.0f);
                    b.AngularVelocity = 0;
                }
            }
            foreach (Body b in _motorACWBodyList)
            {
                if (_flagStartSim) b.ApplyTorque(-1.0f * _torque);
                else
                {
                    b.ApplyTorque(0.0f);
                    b.AngularVelocity = 0;
                }
            }

           //@kester -  update shaft body's rotation & position              
            foreach (GeomPair gp in _shaftGeomPairList)
            {
                Vector2 endPoint = gp.getGeom2Position();
                Vector2 startPoint = gp.getGeom1Position();
                Vector2.Subtract(ref endPoint, ref startPoint, out _shaftDifference);
                CalculateRotation(_shaftDifference);
                gp.update(_shaftRotation, _shaftDifference);
            }

            //@kester - enable/disable body for diff mode.
            foreach (Body b in _obstacles)
            {
                if (_currScreenMode == (int) ScreenMode.VEditor) b.Enabled = false;
                else b.Enabled = true;
            }
            foreach (Body b in _motorCWBodyList)
            {
                if (_currScreenMode == (int)ScreenMode.LEditor) b.Enabled = false;
                else b.Enabled = true;
            }
            foreach (Body b in _motorACWBodyList)
            {
                if (_currScreenMode == (int)ScreenMode.LEditor) b.Enabled = false;
                else b.Enabled = true;
            }
            foreach (Body b in _wheelBodyList)
            {
                if (_currScreenMode == (int) ScreenMode.LEditor) b.Enabled = false;
                else b.Enabled = true;
            }
            foreach (Body b in _shaftBodyList)
            {
                if (_currScreenMode == (int) ScreenMode.LEditor) b.Enabled = false;
                else b.Enabled = true;
            }
                      
           base.Update(gameTime, otherScreenHasFocus, coveredByOtherScreen);
        }
        #endregion

        #region Helper Methods

        #region Joint Rotation Method
        //@kester
        public void CalculateRotation(Vector2 _shaftDifference)
        {
            Vector2.Normalize(ref _shaftDifference, out _normalizedDifference);
            Vector2.Dot(ref _shaftxVector, ref _normalizedDifference, out _shaftTheta);

            _shaftTheta = Calculator.ACos(_shaftTheta);
            if (_shaftDifference.Y < 0)
            {
                _shaftTheta = -_shaftTheta;
            }
            _shaftRotation = _shaftTheta;
        }
        public void CalculateScale(Vector2 _shaftDifference)
        {
            float desiredLength = _shaftDifference.Length();
            _shaftScale.X = desiredLength / _shaftTexture.Width;
            _shaftScale.Y = 1;
        }
        #endregion

        #region Saving & Loading Methods
        //@kester - saving files  
        private void saveToFile()
        {
            //  Level File Format (levels.txt): (Shape 5 & 6 got 2 more parameters)
            //  //Map [MapNumber]
            //  [shape] [xPosition] [yPosition] [isStatic]  [FrictionCoefficient] [activatable]
            //  [shape] [xPosition] [yPosition] [isStatic]  [FrictionCoefficient] [activatable] [verticesCount] [vertices]
            if (_currScreenMode == (int)ScreenMode.LEditor)
            {
                _levelSW.WriteLine("//Map " + _totalMapCount);
                foreach (Geom g in _obstaclesg)
                {
                    _levelSW.Write(g.Body.shape + "\t" + g.Body.Position.X + ", " + g.Body.Position.Y + "\t"
                        + g.Body.IsStatic + "\t" + g.FrictionCoefficient + "\t" + g.Body.activatable);
                    if (g.Body.shape == 5 || g.Body.shape == 6)
                        _levelSW.WriteLine("\t" + g.WorldVertices.Count + "\t" + g.WorldVertices);
                    else
                        _levelSW.WriteLine(); // nextline
                }
                _levelSW.Close();
            }
            else if (_currScreenMode == (int)ScreenMode.VEditor)
            {
                //  Vehicle File Format (vehicles.txt):
                //  //Vehicle [VehicleNumber]
                //  //Wheel	[shape]	[xPosition] [yPosition]	[FrictionCoefficient] [Collision Group] [geomId]
                //  //Motor CW	[shape]	[xPosition] [yPosition]	[FrictionCoefficient] [Collision Group] [geomId]
                //  //Motor ACW	[shape]	[xPosition] [yPosition]	[FrictionCoefficient] [Collision Group] [geomId]
                //  //Shaft [xPosition] [yPosition] [shaftWidth] [shaftRotation] ..
                //           .. [FrictionCoefficient] [CollisionGroup] [ShaftId] [body1Id] [body2Id]
                _vehicleSW.WriteLine("//Vehicle " + _totalVehicleCount);
                foreach (Geom g in _wheelGeomList)
                {
                    _vehicleSW.Write("//Wheel\t");
                    _vehicleSW.WriteLine(g.Body.shape + "\t" + g.Body.Position.X + ", " + g.Body.Position.Y 
                        + "\t" + g.FrictionCoefficient + "\t" + g.CollisionGroup + "\t" + g.GetIndex());                 
                }
                foreach (Geom g in _motorCWGeomList)
                {
                    _vehicleSW.Write("//Motor CW\t");
                    _vehicleSW.WriteLine(g.Body.shape + "\t" + g.Body.Position.X + ", " + g.Body.Position.Y
                        + "\t" + g.FrictionCoefficient + "\t" + g.CollisionGroup + "\t" + g.GetIndex());
                }
                foreach (Geom g in _motorACWGeomList)
                {
                    _vehicleSW.Write("//Motor ACW\t");
                    _vehicleSW.WriteLine(g.Body.shape + "\t" + g.Body.Position.X + ", " + g.Body.Position.Y
                        + "\t" + g.FrictionCoefficient + "\t" + g.CollisionGroup + "\t" + g.GetIndex());
                }
                foreach (Geom g in _shaftGeomList)
                {   
                    //[xPosition] [yPosition] [shaftWidth] [shaftRotation]
                    //[FrictionCoefficient] [CollisionGroup] [ShaftId] [body1Id] [body2Id]
                    _vehicleSW.Write("//Shaft\t");
                    _vehicleSW.WriteLine(g.Body.Position.X + ", " + g.Body.Position.Y + "\t" + g.GetWidth()
                        + "\t" + g.Rotation + "\t" +  g.FrictionCoefficient +"\t" + g.CollisionGroup
                        + "\t" + g.GetIndex() + "\t" + g.GetPairID().X + "\t" + g.GetPairID().Y);                     
                }
                _vehicleSW.Close();
            } // end of vehicle write
        }
        //@kester - load from file
        private void loadFromFile()
        {
            string _currStr;

            if (_currScreenMode == (int)ScreenMode.LEditor) // level editor mode
            {                
                int _currMapID = 1;
                _currStr = _levelSR.ReadLine();

                // clear current lists of obstacles
                _obstacles.Clear();
                _obstaclesg.Clear();

                while (_currStr != null)
                {
                    if (_currStr.StartsWith("//Map")) // load map no.
                    {
                        int _currLength = _currStr.Length - 6;
                        _currMapID = int.Parse(_currStr.Substring(6, _currLength));
                        _currStr = _levelSR.ReadLine();
                    }
                    else if (_mapID == _currMapID) // load items under wanted map
                    {
                        // parse string                    
                        int _currShape;
                        float _x, _y;
                        Vector2 _currPosition;
                        string _currIsStatic;
                        string _currActivatable;
                        int _currVerticesCount;
                        float _currFriction;
                        Vertices _currVertices = new Vertices();
                        float _currVerticesX, _currVerticesY;
                        List<Vertices> _currVerticesList = new List<Vertices>();
                        _currShape = int.Parse(_currStr.Substring(0, _currStr.IndexOf("\t")));
                        _currStr = _currStr.Remove(0, _currStr.IndexOf("\t")).Trim();
                        _x = float.Parse(_currStr.Substring(0, _currStr.IndexOf(",")));
                        _currStr = _currStr.Remove(0, _currStr.IndexOf(",") + 1).Trim();
                        _y = float.Parse(_currStr.Substring(0, _currStr.IndexOf("\t")));
                        _currPosition = new Vector2(_x, _y);
                        _currStr = _currStr.Remove(0, _currStr.IndexOf("\t")).Trim();
                        _currIsStatic = _currStr.Substring(0, _currStr.IndexOf("\t"));
                        _currStr = _currStr.Remove(0, _currStr.IndexOf("\t")).Trim();
                        _currFriction = int.Parse(_currStr.Substring(0, _currStr.IndexOf("\t")));
                        _currStr = _currStr.Remove(0, _currStr.IndexOf("\t")).Trim();

                        if (_currShape == 5 || _currShape == 6)
                        {
                            _currActivatable = _currStr.Substring(0, _currStr.IndexOf("\t"));
                            _currStr = _currStr.Remove(0, _currStr.IndexOf("\t")).Trim();
                            _currVerticesCount = int.Parse(_currStr.Substring(0, _currStr.IndexOf("\t")));
                            _currStr = _currStr.Remove(0, _currStr.IndexOf("\t")).Trim();
                            for (int i = 0; i < _currVerticesCount; ++i)
                            {
                                _currVerticesX = float.Parse(_currStr.Substring(_currStr.IndexOf(":") + 1, _currStr.IndexOf(" ") - _currStr.IndexOf(":")));
                                _currStr = _currStr.Remove(_currStr.IndexOf("{") + 1, _currStr.IndexOf(" ")).Trim();
                                _currVerticesY = float.Parse(_currStr.Substring(_currStr.IndexOf(":") + 1, _currStr.IndexOf("}") - _currStr.IndexOf(":") - 1));
                                _currStr = _currStr.Remove(_currStr.IndexOf("{"), _currStr.IndexOf(" ") + 1);
                                _currVertices.Add(new Vector2(_currVerticesX, _currVerticesY));
                            }
                        }
                        else // not vertices type
                        {
                            _currActivatable = _currStr;
                        }

                        // loading..
                        loadLevelObject(_currShape, _currPosition, _currIsStatic, _currActivatable, _currVertices, _currFriction);

                        _currStr = _levelSR.ReadLine();

                    }
                    else
                    {
                        _currStr = _levelSR.ReadLine();
                    }
                }// end while
                                
                _levelSR.Close();
            } // end of if Level editor mode
            else if (_currScreenMode == (int)ScreenMode.VEditor) // vehicle mode
            {
                int _currVehicleID = 1;
                _currStr = _vehicleSR.ReadLine();

                // clear current lists of 
                _wheelBodyList.Clear();
                _wheelGeomList.Clear();
                _motorACWBodyList.Clear();
                _motorACWGeomList.Clear();
                _motorCWBodyList.Clear();
                _motorCWGeomList.Clear();
                _shaftBodyList.Clear();
                _shaftGeomList.Clear();

                while (_currStr != null)
                {
                    if (_currStr.StartsWith("//Vehicle")) // load vehicle no.
                    {
                        int _currLength = _currStr.Length - 10;
                        _currVehicleID = int.Parse(_currStr.Substring(10, _currLength));
                        _currStr = _vehicleSR.ReadLine();
                    }
                    else if (_vehicleID == _currVehicleID && !_currStr.StartsWith("//Shaft") )
                    //else if (_currStr.StartsWith("//Wheel") && _vehicleID == _currVehicleID) // load wheels
                    {
                        // parse string                    
                        int _currShape, _currCollisionGroup, _currId;
                        float _x, _y;
                        Vector2 _currPosition;
                        float _currFriction;
                        //Wheel	1	213, 483	1	1
                        _currStr = _currStr.Remove(0, _currStr.IndexOf("\t")).Trim();
                        
                        _currShape = int.Parse(_currStr.Substring(0, _currStr.IndexOf("\t")));                        
                        _currStr = _currStr.Remove(0, _currStr.IndexOf("\t")).Trim();
                        _x = float.Parse(_currStr.Substring(0, _currStr.IndexOf(",")));
                        _currStr = _currStr.Remove(0, _currStr.IndexOf(",") + 1).Trim();

                        //_debugSW.WriteLine(_currStr);
                        //_debugSW.Close();
                        _y = float.Parse(_currStr.Substring(0, _currStr.IndexOf("\t")));
                        _currPosition = new Vector2(_x, _y);
                        _currStr = _currStr.Remove(0, _currStr.IndexOf("\t")).Trim();
                        _currFriction = float.Parse(_currStr.Substring(0, _currStr.IndexOf("\t")));
                        _currStr = _currStr.Remove(0, _currStr.IndexOf("\t")).Trim();
                        _currCollisionGroup = int.Parse(_currStr.Substring(0, _currStr.IndexOf("\t")));
                        _currStr = _currStr.Remove(0, _currStr.IndexOf("\t")).Trim();
                        _currId = int.Parse(_currStr); 
                        
                        // loading...
                        loadVehicleObject(_currShape, _currPosition, _currFriction, _currCollisionGroup,
                                        _currId, 0, 0, 0, 0, 0);
                        _currStr = _vehicleSR.ReadLine();
                    }
                    else if (_currStr.StartsWith("//Shaft")) // load shaft
                    {
                        //[xPosition] [yPosition] [shaftWidth] [shaftRotation]	[FrictionCoefficient] [CollisionGroup] [ShaftId]
                        //Shaft	353, 502.5	288.0017	3.701664	1	1	2	4	1
                        // parse string
                        float _x, _y, _currWidth, _currRot, _currFriction;
                        int _currCollisionGroup, _currShaftId, _currBody1Id, _currBody2Id;
                        Vector2 _currPosition;

                        _x = float.Parse(_currStr.Substring(8, _currStr.IndexOf(",") - 8));
                        _currStr = _currStr.Remove(0, _currStr.IndexOf(",") + 1).Trim();                        
                        _y = float.Parse(_currStr.Substring(0, _currStr.IndexOf("\t")));
                        _currStr = _currStr.Remove(0, _currStr.IndexOf("\t") + 1).Trim();
                        _currWidth = float.Parse(_currStr.Substring(0, _currStr.IndexOf("\t")));
                        _currStr = _currStr.Remove(0, _currStr.IndexOf("\t") + 1).Trim();
                        _currRot = float.Parse(_currStr.Substring(0, _currStr.IndexOf("\t")));
                        _currStr = _currStr.Remove(0, _currStr.IndexOf("\t") + 1).Trim();
                        _currFriction = float.Parse(_currStr.Substring(0, _currStr.IndexOf("\t")));
                        _currStr = _currStr.Remove(0, _currStr.IndexOf("\t") + 1).Trim();
                        _currCollisionGroup = int.Parse(_currStr.Substring(0, _currStr.IndexOf("\t")));
                        _currStr = _currStr.Remove(0, _currStr.IndexOf("\t") + 1).Trim();
                        _currShaftId = int.Parse(_currStr.Substring(0, _currStr.IndexOf("\t")));
                        _currStr = _currStr.Remove(0, _currStr.IndexOf("\t") + 1).Trim();
                        _currBody1Id = int.Parse(_currStr.Substring(0, _currStr.IndexOf("\t")));
                        _currStr = _currStr.Remove(0, _currStr.IndexOf("\t") + 1).Trim();
                        _currBody2Id = int.Parse(_currStr);

                        _currPosition = new Vector2(_x, _y);
                        //_debugSW.WriteLine(_x + " " + _y + " " +  _currWidth + " " +  _currRot + " " + 
                          //  _currFriction + " " +  _currCollisionGroup + " " +  _currShaftId + " " + 
                            //_currBody1Id + " " + _currBody2Id);
                        
                        // loading...
                        loadVehicleObject(0, _currPosition, _currFriction, _currCollisionGroup, 0,
                                      _currWidth, _currRot, _currShaftId, _currBody1Id, _currBody2Id);
                        _currStr = _vehicleSR.ReadLine();
                    }
                    else
                    {
                        _currStr = _vehicleSR.ReadLine();
                    }
                }

                _vehicleSR.Close();

            } // end of if Vehicle editor screen mode
            //_debugSW.Close();
        }
        
        //@kester - load level's objects
        private void loadLevelObject(int tShape, Vector2 tPosition, string tIsStatic, string tActivatable, Vertices tVertices, float _tFriction)
        {    
            if (tShape == 1)
            {
                Body _currBody = BodyFactory.Instance.CreateRectangleBody(PhysicsSimulator, 40, 40, 25);
                _obstacles.Add(_currBody);
                _currBody.shape = tShape;
                _currBody.Position = tPosition;
                if (tIsStatic.Equals("True")) _currBody.IsStatic = true;
                else _currBody.IsStatic = false;
                
                Geom _currGeom = GeomFactory.Instance.CreateRectangleGeom(PhysicsSimulator, _obstacles[_obstacles.Count - 1], 40, 40);
                _currGeom.FrictionCoefficient = _tFriction;
                _currGeom.SetBody(_currBody);
                _obstaclesg.Add(_currGeom);
                if (tActivatable.Equals("True"))
                {
                    _currBody.activatable = true;
                    _obstaclesg[_obstacles.Count - 1].OnCollision += OnCollision;
                    PhysicsSimulator.BroadPhaseCollider.OnBroadPhaseCollision += OnBroadPhaseCollision;
                }
                else
                {
                    _currBody.activatable = false;
                }    
                 
            }

            if (tShape == 2)
            {                
                Body _currBody = BodyFactory.Instance.CreateCircleBody(PhysicsSimulator, 20, 30);
                _obstacles.Add(_currBody);
                _currBody.shape = tShape;
                _currBody.Position = tPosition;
                if (tIsStatic.Equals("True")) _currBody.IsStatic = true;
                else _currBody.IsStatic = false;
                Geom _currGeom = GeomFactory.Instance.CreateCircleGeom(PhysicsSimulator, _obstacles[_obstacles.Count - 1], 20, 30);
                _currGeom.FrictionCoefficient = _tFriction;
                _currGeom.SetBody(_currBody);
                _obstaclesg.Add(_currGeom);
                if (tActivatable.Equals("True"))
                {
                    _currBody.activatable = true;
                    _obstaclesg[_obstacles.Count - 1].OnCollision += OnCollision;
                    PhysicsSimulator.BroadPhaseCollider.OnBroadPhaseCollision += OnBroadPhaseCollision;
                }
                else
                {
                    _currBody.activatable = false;
                }
            }

            else if (tShape == 3)
            {                                
                Body _currBody = BodyFactory.Instance.CreateRectangleBody(PhysicsSimulator, 20, 80, 30);
                _obstacles.Add(_currBody);
                _currBody.shape = tShape;
                _currBody.Position = tPosition;
                if (tIsStatic.Equals("True")) _currBody.IsStatic = true;
                else _currBody.IsStatic = false;                
                Geom _currGeom = GeomFactory.Instance.CreateRectangleGeom(PhysicsSimulator, _obstacles[_obstacles.Count - 1], 20, 80);
                _currGeom.FrictionCoefficient = _tFriction;
                _currGeom.SetBody(_currBody);
                _obstaclesg.Add(_currGeom);
                if (tActivatable.Equals("True"))
                {
                    _currBody.activatable = true;
                    _obstaclesg[_obstacles.Count - 1].OnCollision += OnCollision;
                    PhysicsSimulator.BroadPhaseCollider.OnBroadPhaseCollision += OnBroadPhaseCollision;
                }
                else
                {
                    _currBody.activatable = false;
                }
            }

            else if (tShape == 4)
            {
                Body _currBody = BodyFactory.Instance.CreateRectangleBody(PhysicsSimulator, 80, 20, 30);
                _obstacles.Add(_currBody);
                _currBody.shape = tShape;
                _currBody.Position = tPosition;
                if (tIsStatic.Equals("True")) _currBody.IsStatic = true;
                else _currBody.IsStatic = false;
                Geom _currGeom = GeomFactory.Instance.CreateRectangleGeom(PhysicsSimulator, _obstacles[_obstacles.Count - 1], 80, 20);
                _currGeom.FrictionCoefficient = _tFriction;
                _currGeom.SetBody(_currBody);
                _obstaclesg.Add(_currGeom);
                if (tActivatable.Equals("True"))
                {
                    _currBody.activatable = true;
                    _obstaclesg[_obstacles.Count - 1].OnCollision += OnCollision;
                    PhysicsSimulator.BroadPhaseCollider.OnBroadPhaseCollision += OnBroadPhaseCollision;
                }
                else
                {
                    _currBody.activatable = false;
                }
            }

            else if (tShape == 5)
            {
                Body _currBody = BodyFactory.Instance.CreatePolygonBody(PhysicsSimulator, tVertices, 5);               
                _obstacles.Add(_currBody);
                _currBody.shape = tShape;
                _currBody.Position = tPosition;
                if (tIsStatic.Equals("True")) _currBody.IsStatic = true;
                else _currBody.IsStatic = false;
                Geom _currGeom = GeomFactory.Instance.CreatePolygonGeom(PhysicsSimulator, _obstacles[_obstacles.Count - 1], tVertices, 0);
                _currGeom.FrictionCoefficient = _tFriction;
                _currGeom.SetBody(_currBody);
                _obstaclesg.Add(_currGeom);
                if (tActivatable.Equals("True"))
                {
                    _currBody.activatable = true;
                    _obstaclesg[_obstacles.Count - 1].OnCollision += OnCollision;
                    PhysicsSimulator.BroadPhaseCollider.OnBroadPhaseCollision += OnBroadPhaseCollision;
                }
                else
                {
                    _currBody.activatable = false;
                }
            }

            else if (tShape == 6)
            {
                Body _currBody = BodyFactory.Instance.CreatePolygonBody(PhysicsSimulator, tVertices, 5);
                _obstacles.Add(_currBody);
                _currBody.shape = tShape;
                _currBody.Position = tPosition;
                if (tIsStatic.Equals("True")) _currBody.IsStatic = true;
                else _currBody.IsStatic = false;
                Geom _currGeom = GeomFactory.Instance.CreatePolygonGeom(PhysicsSimulator, _obstacles[_obstacles.Count - 1], tVertices, 0);
                _currGeom.FrictionCoefficient = _tFriction;
                _currGeom.SetBody(_currBody);
                _obstaclesg.Add(_currGeom);
                if (tActivatable.Equals("True"))
                {
                    _currBody.activatable = true;
                    _obstaclesg[_obstacles.Count - 1].OnCollision += OnCollision;
                    PhysicsSimulator.BroadPhaseCollider.OnBroadPhaseCollision += OnBroadPhaseCollision;
                }
                else
                {
                    _currBody.activatable = false;
                }
            }
        }
        //@kester - load vehicle's objects
        private void loadVehicleObject(int tShape, Vector2 tPosition, float tFriction, int tCollisionGroup,
                              int tId, float tWidth, float tRotation, int tShaftId, int tBody1Id,  int tBody2Id)
        {
            if (tShape == 1)
            {
                Body _currBody = BodyFactory.Instance.CreateCircleBody(PhysicsSimulator, 34, 10);                
                _currBody.shape = tShape;
                _currBody.Position = tPosition;
                _currBody.IsStatic = true;
                _wheelBodyList.Add(_currBody);

                Geom _currGeom = GeomFactory.Instance.CreateCircleGeom(PhysicsSimulator, _wheelBodyList[_wheelBodyList.Count - 1], 34, 34);
                _currGeom.FrictionCoefficient = tFriction;
                _currGeom.CollisionGroup = tCollisionGroup;
                _currGeom.SetIndex(tId);
                _wheelGeomList.Add(_currGeom); 
            }
            else if (tShape == 2)
            {
                Body _currBody = BodyFactory.Instance.CreateCircleBody(PhysicsSimulator, 34, 10);
                _currBody.shape = tShape;
                _currBody.Position = tPosition;
                _currBody.IsStatic = true;
                _motorCWBodyList.Add(_currBody);

                Geom _currGeom = GeomFactory.Instance.CreateCircleGeom(PhysicsSimulator, _motorCWBodyList[_motorCWBodyList.Count - 1], 34, 34);
                _currGeom.FrictionCoefficient = tFriction;
                _currGeom.CollisionGroup = tCollisionGroup;
                _currGeom.SetIndex(tId);
                _motorCWGeomList.Add(_currGeom);
            }
            else if (tShape == 3)
            {
                Body _currBody = BodyFactory.Instance.CreateCircleBody(PhysicsSimulator, 34, 10);
                _currBody.shape = tShape;
                _currBody.Position = tPosition;
                _currBody.IsStatic = true;
                _motorACWBodyList.Add(_currBody);

                Geom _currGeom = GeomFactory.Instance.CreateCircleGeom(PhysicsSimulator, _motorACWBodyList[_motorACWBodyList.Count - 1], 34, 34);
                _currGeom.FrictionCoefficient = tFriction;
                _currGeom.CollisionGroup = tCollisionGroup;
                _currGeom.SetIndex(tId);
                _motorACWGeomList.Add(_currGeom);
            } 
            else    // shaft
            {
                Body _currBody = BodyFactory.Instance.CreateRectangleBody(PhysicsSimulator, tWidth, 15, 10);
                _currBody.Rotation = tRotation;
                _currBody.Position = tPosition;
                _shaftBodyList.Add(_currBody);

                Geom _currGeom1 = new Geom(); 
                Geom _currGeom2 = new Geom();

               
                foreach (Geom gWheel in _wheelGeomList)// wheel's index matches shaft's bodyId
                {
                     if (gWheel.GetIndex() == tBody1Id) 
                          _currGeom1 = gWheel;
                     if (gWheel.GetIndex() == tBody2Id) 
                         _currGeom2 = gWheel;
                }
                foreach (Geom gMotorCW in _motorCWGeomList)// motorCW's index matches shaft's bodyId
                {
                    if (gMotorCW.GetIndex() == tBody1Id)
                        _currGeom1 = gMotorCW;
                    if (gMotorCW.GetIndex() == tBody2Id)
                        _currGeom2 = gMotorCW;
                }
                foreach (Geom gMotorACW in _motorACWGeomList)// motorACW's index matches shaft's bodyId
                {
                    if (gMotorACW.GetIndex() == tBody1Id)
                        _currGeom1 = gMotorACW;
                    if (gMotorACW.GetIndex() == tBody2Id)
                        _currGeom2 = gMotorACW;
                }                    
               

                Vector2 endPoint = _currGeom2.Position;
                Vector2 startPoint = _currGeom1.Position;
                Vector2.Subtract(ref endPoint, ref startPoint, out _shaftDifference);
                CalculateRotation(_shaftDifference);

                _jointRev = JointFactory.Instance.CreateRevoluteJoint(PhysicsSimulator, _currGeom1.Body, _currBody, _currGeom1.Body.Position);
                _jointRevList.Add(_jointRev);
                _jointRev = JointFactory.Instance.CreateRevoluteJoint(PhysicsSimulator, _currBody, _currGeom2.Body, _currGeom2.Body.Position);
                _jointRevList.Add(_jointRev);

                Geom _currGeom = GeomFactory.Instance.CreateRectangleGeom(PhysicsSimulator, _currBody, tWidth, 10);
                _currGeom.CollisionGroup = tCollisionGroup;
                _currGeom.SetBody(_currBody);
                _currGeom.SetPairID(tBody1Id, tBody2Id);
                _currGeom.SetWidth(tWidth);
                _currGeom.SetIndex(_shaftGeomList.Count + 1);
                _currGeom.FrictionCoefficient = tFriction;
                //_currGeom.SetRotation(_shaftRotation);
                _shaftGeomList.Add(_currGeom);

                GeomPair gp = new GeomPair(_currGeom1, _currGeom2);
                gp.setDifference(_shaftDifference);
                gp.setRotation(tRotation);
                gp.setOrigin(new Vector2(tWidth / 2, 5));
                _shaftGeomPairList.Add(gp);
                _shaftID = _shaftGeomPairList.Count;
            }
        }

        
        #endregion  //end saving & loading methods

        //@Daniel
        private bool OnCollision(Geom geom1, Geom geom2, ContactList contactList)
        {
            if (geom1.Body.activatable == true)
                geom1.Body.IsStatic = false;
            if (geom2.Body.activatable == true)
                geom2.Body.IsStatic = false;
            return true;
        }

        private bool OnBroadPhaseCollision(Geom geom1, Geom geom2)
        {
            return true;
        }

        #endregion // end helper methods

        #region GetTitle Method
        public static string GetTitle()
        {
            return "Editor";
        }
        #endregion

        #region GetDetails Method
        public static string GetDetails()
        {
            StringBuilder sb = new StringBuilder();
            sb.AppendLine("Level Editor");
            sb.AppendLine("PageUp or PageDown to change object");
            sb.AppendLine("on editor brush.");
            sb.AppendLine("Click to add objects.");
            sb.AppendLine("Press S to toggle between");
            sb.AppendLine("static and free objects.");
            sb.AppendLine(string.Empty);
            sb.AppendLine("Keyboard:");
            sb.AppendLine("  -Move : left and right arrows");
            return sb.ToString();
        }
        #endregion
    }
}
